#include "LightPosDetector.h"
using namespace cv;
#define THRESHOLD 1

LightPosDetector::LightPosDetector ()
{
  _pCamera = cvCaptureFromCAM (0);
  cvSetCaptureProperty (_pCamera, CV_CAP_PROP_FRAME_WIDTH, 640);
  cvSetCaptureProperty (_pCamera, CV_CAP_PROP_FRAME_HEIGHT, 480);
  grey_img = cvCreateImage (cvSize (640, 480), IPL_DEPTH_8U, 1);
  t_img = cvCreateImage (cvSize (640, 480), IPL_DEPTH_8U, 1);
}


LightPosDetector::~LightPosDetector ()
{
  cvReleaseImage (&grey_img);
  cvReleaseImage (&t_img);
  cvReleaseCapture (&_pCamera);
}


bool LightPosDetector::get_point_input (OUT Array< float >& pointCoord)
{
  if (pointCoord.size () != 2)
    pointCoord.resize (2);

  // Find the light pos
  CvPoint lightPos = _find_light_pos ();

  if (lightPos.x >= 0 && lightPos.y >= 0) // Light pos was found
  {
    pointCoord[X] = lightPos.x;
    pointCoord[Y] = lightPos.y;
    return true;
  }
  else
  {
    pointCoord[X] = -1.0f;
    pointCoord[Y] = -1.0f;
    return false;
  }
}




CvPoint LightPosDetector::_find_light_pos () const
{
  // Get frame
  IplImage* image = cvQueryFrame (_pCamera);
  if (image == NULL)
  throw ("Init Camera Fail");

  // Convert to black-white image
  cvCvtColor (image, grey_img, CV_BGR2GRAY);
  cvThreshold (grey_img, t_img, THRESHOLD, 255, CV_THRESH_BINARY);

  cvSaveImage( "test.bmp" , t_img);
  // For debug
  // cvShowImage ("video", t_img);
  // printf ("%d %d\n", max_loc.x, max_loc.y);

  //cvReleaseImage (&image);
  
  static CvMemStorage *storage = cvCreateMemStorage (0);
  CvSeq * contour;
  int count = 0;
  CvPoint center;
  center.x = 0; center.y = 0;
  cvFindContours( t_img, storage, &contour, sizeof (CvContour),
    CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);

  for(; contour; contour = contour->h_next){
    CvRect rect = cvBoundingRect(contour, 1);

    CvPoint cur_center=cvPoint(rect.x+rect.width/2, rect.y+rect.height/2);
    ++count;
    center.x += cur_center.x;
    center.y += cur_center.y;

  }
  if (count)
  {
    center.x /= count;
    center.y /= count;
    return center;
  }
  else
    return cvPoint(-1,-1);
}